Velocity control method and apparatus for servo motors

ABSTRACT

A velocity control method for use in a servo motor having a disturbance eliminator is described. The velocity control method includes the steps of calculating a control torque input value based on a desired target velocity input value and an output velocity value of an actual mechanical model, adding a disturbance torque value to the control torque input value to input the added result to the mechanical model, inputting the control torque input value to a virtual model to calculate an estimation velocity according to the control torque input value, calculating a corrected torque based on a velocity value difference between the output velocity value of the mechanical model and the estimation velocity value of the virtual model, and reflecting the corrected torque on the control torque input value. Accordingly, although unknown disturbances are input to the control system, a velocity error can be conversed into zero as closely as possible to enable an accurate velocity control.

BACKGROUND OF THE INVENTION

The present invention relates to a velocity control method for a servo motor and a velocity control apparatus adopting the same. A controller for calculating a control torque from a preset input value and a feedback portion for feeding back the output of an object to be controlled and comparing the fedback output with the input value are needed to automatically obtain a desired output from a servo motor or a robot which is the object to be controlled.

Meanwhile, an electrical disturbance due to noise or an electromagnetic wave, and a mechanical disturbance due to vibration or dynamic characteristics affect a control system in an actual system such as a robot or a machining tool. Such a disturbance is added to or subtracted from a control torque input or a feedback signal to thereby deteriorate a response performance and to further instabilize the system. Thus, a disturbance eliminator or a disturbance estimator is designed to minimize an influence due to such a disturbance.

FIG. 3 is a block diagram of a conventional control system having a servo motor and a disturbance eliminator. An input torque τ* necessary to a desired velocity is converted into an actual current i corresponding to input torque τ* and then the converted current i is applied to a servo motor 10. Then, a torque constant block 6 in servo motor 10 generates a torque T_(u) in proportional to applied current i. A linear relationship between applied current i and generated torque T_(u) is expressed as a torque constant K_(t). When generated torque T_(u) drives a rotator of servo motor 10, assuming that J is a moment of inertia, B is a viscous friction coefficient and s is a Laplacian variable, a dynamic characteristic is expressed as 1/(Js+B). Such a torque constant K_(t) is represented in torque constant block 6 and the dynamic characteristic of the rotator is represented in dynamic characteristic block 7.

The above-described control system represents an ideal circumstance, but an actual control system is affected by various electrical or mechanical disturbance so that a response performance is lowered. To consider such various disturbances in designing a control system, it is assumed that a disturbance T_(L) is applied between torque constant block 6 and dynamic characteristic block 7.

In this case, an output ω of dynamic characteristic block 7 affected by the disturbance is different from that when there is no disturbance T_(L). Thus, output ω of dynamic characteristic block 7 affected by the disturbance T_(L) becomes important information to predict disturbance T_(L). Therefore, a disturbance estimator 8 estimates an actually applied disturbance T_(L) from corrected current i and output ω of dynamic characteristic block 7. The estimated disturbance T_(L) is fedback to the input side of torque constant block 6 and added to input torque τ* in an adder 13 to increase current i and linearly increase generated torque T_(u) by an amount of the increase of current i. Accordingly, although a negative disturbance T_(L) is applied to generated torque T_(u) in an adder 15, an amount of the increase of generated torque T_(u) are cancelled each other, thereby unchanging an output velocity ω. An effect of disturbance T_(L) can be removed by using disturbance estimator 8.

However, since a disturbance elimination method using such a disturbance estimator does not operate on a real time basis, if a high-frequency disturbance is applied thereto, an estimation speed does not follow a changing speed of the high-frequency disturbance, with a result that the disturbance is not eradicated. Also, since the disturbance estimator is positioned in a feedback portion, an entire system becomes a high-gain controller by a gain adjustment of the disturbance estimator and is apt to be instabilized accordingly. Also, an internal gain value in the disturbance estimator is involved in a control system variable, to thereby make it difficult to design the control system.

SUMMARY OF THE INVENTION

Therefore, to solve the above problem, it is an object of the present invention to provide a velocity control method which makes it possible to converge a control error into zero as closely as possible all the time irrespective of a disturbance applied to a servo motor.

To accomplish the above object of the present invention, there is provided a velocity control method for use in a servo motor having a disturbance eliminator, the velocity control method comprising the steps of: calculating a control torque input value based on a desired target velocity input value and an output velocity value of an actual mechanical model; adding a disturbance torque value to the control torque input value to input the added result to the mechanical model; inputting the control torque input value to a virtual model to calculate an estimation velocity according to the control torque input value; calculating a corrected torque based on a velocity value difference between the output velocity value of the mechanical model and the estimation velocity value of the virtual model; and reflecting the corrected torque on the control torque input value.

Preferably, the control torque input value calculation step comprises the step of obtaining a proportional control input value by multiplying a difference value between the output velocity value of the mechanical model and the target velocity input value by a proportional constant.

Preferably, wherein the control torque input value calculation step further comprises the steps of: obtaining a feed forward control input value by multiplying a differentiation value of the target velocity input value by a feed forward constant; and obtaining the control torque input value by adding the feed forward control input value and the proportional control input value.

A relationship between an input torque and an output torque in the mechanical model is expressed as the following equation:

    τ*(t)=J.sub.n ·ω(t)+.increment.J(t)·ω(t)+J(t)·.omega.(t)+F.sub.f +T.sub.d

in which τ*(t) is an input torque, J_(n) is a nominal moment of inertia, ω(t) is an angular velocity, J(t) is a moment of inertia which is time-varying according to a load, .increment.J(t) is expressed as J_(n) -J(t), F_(f) is a frictional torque and T_(d) is a disturbance torque.

The estimation velocity in the virtual model is expressed as the following equation: ##EQU1## in which ω(s) is an estimation velocity, T_(u) (s) is a control torque, and s is a Laplacian variable.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG. 1 is a block diagram of a control system according to the present invention.

FIG. 2 is a graphical view showing a response characteristic of the control system in FIG. 1.

FIG. 3 is a block diagram of a conventional control system having a servo motor and a disturbance controller.

FIG. 4 is a graphical view showing a response characteristic of the control system in FIG. 3.

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENT

A preferred embodiment of the present invention will be described below in more detail with reference to the accompanying drawings FIGS. 1 and 2.

In a control system of FIG. 1 according to the present invention, a controller largely includes a proportional controller 5 and a feed forward controller 4. The proporational controller 5 calculates a torque necessary for a servo motor to reach a desired velocity. This is because a linear relationship is established between a velocity error ω_(e) and generated torque T₁ within the boundary of a predetermined operating velocity in the servo motor. A slope of a straight line representing the linear relationship is a velocity proportional gain K_(vp) representing proportional controller 5.

Feed forward controller 4 differentiates a target velocity input value ω_(d) * to obtain an angular accelerating velocity. The angular accelerating velocity is multiplied by a feed forward constant, i.e., a moment of inertia J_(n) to calculate a feed forward control input T₂. Since the feed forward controller 4 includes a differentiator (s), if target velocity input value ω_(d) * is invariable, a value of feed forward control input T₂ becomes zero, to accordingly provide an effect that feed forward controller 4 does not operate. To the contrary, in case of a tracking control where target velocity input value ω_(d) * is time-varying, feed forward controller 4 calculates an additional control input T₂ necessary for tracking target velocity input value ω_(d) *. This is because only by a proportional controler 5, the controller cannot track a parabolic input not less than a ramp input.

The outputs of proportional controller 5 and feed forward controller 4 are added in an adder 14 to produce a control torque input value T_(u), and then added to a disturbance torque T_(L) in another adder 16. Disturbance T_(L) is added to a corrected torque τ₁ between control torque input T_(u) and the input of a mechanical model 1. Here, disturbance T_(L) is positively added to control torque input T_(u) and corrected torque τ₁ is negatively added to control torque input T_(u).

The adder 16 outputs input torque τ* to the input end of mechanical model 1. The mechanical model of the actual servo motor is expressed as the following equation (1).

    τ*(t)=J.sub.n ·ω(t)'+.increment.J(t)·ω(t)+J(t)·.omega.(t)+F.sub.f +T.sub.d                                  (1)

Here, τ*(t) is an input torque, J_(n) is a nominal moment of inertia, ω(t) is an angular velocity, J(t) is a moment of inertia which is time-varying according to a load, .increment.J(t) is expressed as J_(n) -J(t), F_(f) is a frictional torque and T_(d) is a disturbance torque.

Meanwhile, control torque input value T_(u) is distributed in two directions, in which one is directed to mechanical model 1 and the other is directed to a virtual model 2. Virtual model 2 is designed by approximately estimating mechanical model 1, and expressed as the following equation (2). ##EQU2##

This is because an accurate modeling of mechanical model 1 is difficult and it is more convenient and effective to use a simple estimator than making an effort to obtain an accurate modeling. Here, ω(s) is an output velocity of virtual model 2, T_(u) (s) is a control torque, and s is a Laplacian variable.

A correction torque calculator 3 is constructed with a single torque conversion constant K and calculates a corrected torque τ₁ in proportional to a difference between output velocity ω of mechanical model 1 and output velocity ω of virtual model 2 via adder 18, as in proportional controller 5.

By the above construction, an operating principle of the present invention will be described and the equation of mechanical model 1 and necessary proof thereof will be described later on.

If a desired target velocity input ω_(d) * is applied to the input end of the system, target velocity input ω_(d) * is differentiated by feed forward controller 4 and amplified by a forward constant J_(n), to calculate feed forward control input T₂.

An adder 12 outputs a difference between target velocity input ω_(d) * and fedback velocity output ω as a velocity error ω_(e) to proportional controller 5 which calculates a proportional control input T₁. The feed forward control input T₂ and proportional control input T₁ are added in adder 14 to produce control torque input T_(u).

Then, control torque input T_(u) is branched to the input side of mechanical model 1 and the input end of virtual model 2.

Adder 16 adds disturbance T_(L) to control torque input T_(u) directed to mechanical model 1 and subtracts corrected torque τ₁ output from correction torque calculator 3 which is a feedback portion from control torque input T_(u), prior to being input to mechanical model 1, to finally produce input torque τ*. Mechanical model 1 starts to rotate according to input torque τ* to output a velocity ω.

The other branched control torque input T_(u) is input to virtual model 2, and estimates an output velocity ω of mechanical model 1 which is not influenced by disturbance T_(L), to calculate an estimation velocity ω. Adder 18 subtracts estimation velocity ω(s) from output velocity ω to calculate a velocity difference which is input to correction torque calculator 3.

Correction torque calculator 3 receives the input velocity difference and produces corrected torque τ₁ in proportional to torque conversion constant K, to then be fedback to adder 16. The fedback corrected torque τ₁ is negatively added in adder 16 unlike disturbance T_(L). Accordingly, an effect of disturbance T_(L) is cancelled with that of corrected torque τ₁ to thereby provide an effect of removing disturbance T_(L).

The output velocity ω of mechanical model 1 forms a unit feedback loop and is fedback between target velocity input ω_(d) * and velocity error ω_(e) to play a role of reducing an error between target velocity input ω_(d) * and velocity error ω.

The above-described system can be implemented by installing an actual servo motor or an arm of a robot in the position where mechanical model 1 is located. Besides, the other blocks are implemented in a control program by software and then interfaced using a velocity sensor and a digital-to-analog converter amd an analog-to-digital converter.

By using the above-described consturction of FIG. 1, the velocity of the actual servo motor is controlled, of which the control result is shown in FIG. 2. Meanwhile, the velocity of the actual servo motor is controlled using the conventional control system having a disturbance eliminator 8 shown in FIG. 3, in which the control result is shown in FIG. 4.

Referring to FIGS. 2 and 4, the effect of the present invention is compared with that of the conventional art. Referring to FIG. 4, whenever disturbance T_(L) is applied, disturbance eliminator 8 is not effectively cancelled and reflected on output velocity ω, to provide an inconsistent response. Meanwhile, referring to FIG. 2, although an identical disturbance T_(L) is applied, correction torque calculator 3 which performs a function of a disturbance eliminator cancels disturbance T_(L) on a real time basis, so that the effect of the disturbance is not reflected on output velocity ω. Thus, output velocity ω has a consistent response performance.

As described above, although unknown disturbances are input to a control system, the present invention can converge the velocity error into zero as closely as possible to thereby enable an accurate velocity control.

Hereinbelow, the present invention will be expressed in the equations and the necessary proofs thereof will be given.

First, mechanical model 1 in a time domain is expressed as the above-described equation (1).

    τ*(t)=J.sub.n ·ω(t)+.increment.J(t)·ω(t)+J(t)·.omega.(t)+F.sub.f +T.sub.d                                   (1)

An input torque τ* is expressed as the following equation (3) using the control system of FIG. 1.

    τ*(t)=J.sub.n ·ω.sub.e (t)+K.sub.vp ·ω.sub.e (t)-τ.sub.1 (t)               (3)

Here, ω_(e) (t)=ω_(d) (t)-ω(t).

Using equations (1) and (3), τ*(t) is simply expressed as the following equation (4).

    τ*(t)=J.sub.n ·ω.sub.d (t)+τ.sub.d (t)(4)

Here, τ_(d) (t)=.increment.J(t)·ω(t)+J(t)·ω(t)+F_(f) +T_(d)

Then, mechanical model and the disturbance eliminator in the srvo motor are modelled as following equations (5) and (6). ##EQU3##

    τ.sub.1 (t)=K{ω(t)-ω(t)}                   (6)

Then, for the performance analysis, we define the following expressions (7), (8) and (9). ##EQU4##

    e.sub.2 (t)=e.sub.1 (t)                                    (8)

    {|J(t)|≦d.sub.1, |.increment.J(t)|≦d.sub.2, |T.sub.d |≦d.sub.3, |F.sub.f |≦d.sub.4, |ω.sub.d (t)|≦d.sub.5, |ω.sub.d (t)|≦d.sub.6      (9)

Using the expressions (7), (8) and (9), the present invention is proved as the following proof. Accordingly, it can be seen that the velocity control error becomes always zero irrespective of the presence of the disturbance.

Proof

Various constants are defined as follows.

    b.sub.1 >1/2+δ.sub.1,                                (10)

    δ.sub.1 >0                                           (11)

    Υ.tbd.(b.sub.1 -1/2δ.sub.1)/{b.sub.1.sup.2 +d.sub.1 /(J.sub.n -d.sub.2)}                                                (12)

    K.sub.vp .tbd.J.sub.n ·K/(K-J.sub.n)              (13)

    δ.sub.2 .tbd.Υ(√Υ+b.sub.1 Υ)d/{√2(J.sub.n -d.sub.2)}, ε>0    (14)

    ε>0                                                (15)

    K>ε                                                (16)

Here,

    d.tbd.d.sub.1 d.sub.5 +d.sub.2 d.sub.6 +d.sub.3 +d.sub.4   (17)

    ε.tbd.(J.sub.n +d.sub.2){(δ.sub.2 +1/2)/Υ+b.sub.1.sup.2 /2+3d.sub.1 /2(J.sub.n -d.sub.2)+b.sub.1 }(18)

Then, ξ, d and V which is called the Lyapunov function are determined as the following equations (19) through (21).

    ξ.tbd.e.sub.2 +b.sub.1 e.sub.1                          (19)

    d.tbd.J(t)ω.sub.d +.increment.J(t)·ω.sub.d +T.sub.d +F.sub.f                                                  (20)

    V.tbd.(|e.sub.1 |.sup.2 +Υ|ξ|.sup.2)/2               (21)

Then, the following equation (22) is satisfied. ##EQU5## Here,

    K.sub.1 .tbd.KK.sub.vp /J.sub.n                            (23)

    K.sub.2 .tbd.K+K.sub.vp                                    (24)

By equation (9), equation (22) is expressed as equation (25).

    V≦-δ.sub.1 e.sub.1.sup.2 -δ.sub.2 ξ.sup.2 +|ξ|Υd/(J.sub.n -d.sub.2)=-δ.sub.1 e.sub.1.sup.2 -δ.sub.2 |ξ|{|ξ|-Υd/(J.sub.n -d.sub.2)δ.sub.2 }.                                 (25)

Then, the following equations (26) and (27) are defined.

    e.sub.1 .tbd.e.sub.1 /√Υ                    (26)

    δ.tbd.Υd/(J.sub.n -d.sub.2)δ.sub.2     (27)

Thus, the Lyapunov function of equation (21) can be expressed as the following equation (28).

    V.tbd.Υ(|e.sub.1 |.sup.2 +|ξ|.sup.2)/2                        (28)

Here, if

    |e.sub.1 |.sup.2 +|ξ|.sup.2 >δ.sup.2 V<0                                        (29)

Thus, T>0, and T satisfying the following inequality (30) exists. ##EQU6## That is, the following inequality (31) is satisfied.

    |ξ|≦√Υδ/√ω.omega./, |e.sub.1 |≦√2, ∀t≧T(31)

From equation (21) and inequality (31) the following inequality (32) is satisfied.

    |e.sub.2 |≦(√Υb.sub.1 Υ)δ/√2<ε, ∀t≧T(32)

Also, since |ξ|≧|e₂ |-b₁ |e₁ |, the following inequality is satisfied.

    |e.sub.1 |≦Υδ/√2, ∀t≧T                                    (33)

Thus, the velocity error is always small enough within the bounds desired.

Proof end 

What is claimed is:
 1. A velocity control method for use in a servo motor having a disturbance eliminator, said velocity control method comprising the steps of:calculating a control torque input value based on a desired target velocity input value and an output velocity value of an actual mechanical model; adding a disturbance torque value to the control torque input value to input the added result to said mechanical model; inputting the control torque input value to a virtual model to calculate an estimation velocity according to the control torque input value; calculating a corrected torque based on a velocity value difference between the output velocity value of said mechanical model and the estimation velocity value of said virtual model; and reflecting the corrected torque on the control torque input value.
 2. A velocity control method for use in a servo motor according to claim 1, wherein said control torque input value calculation step comprises the step of obtaining a proportional control input value by multiplying a difference value between the output velocity value of said mechanical model and the target velocity input value by a proportional constant.
 3. A velocity control method for use in a servo motor according to claim 2, wherein said control torque input value calculation step further comprises the steps of:obtaining a feed forward control input value by multiplying a differentiation value of the target velocity input value by a feed forward constant; and obtaining the control torque input value by adding the feed forward control input value and the proportional control input value.
 4. A velocity control method for use in a servo motor according to claim 1, wherein a relationship between an input torque and an output torque in said mechanical model is expressed as the following equation:

    τ*(t)=J.sub.n ·ω(t)+.increment.J(t)·ω(t)+J(t)·.omega.(t)+F.sub.f +T.sub.d

in which τ*(t) is an input torque, J_(n) is a nominal moment of inertia, ω(t) is an angular velocity, J(t) is a moment of inertia which is time-varying according to a load, .increment.J(t) is expressed as J_(n) -J(t), F_(f) is a frictional torque and T_(d) is a disturbance torque.
 5. A velocity control method for use in a servo motor according to claim 2, wherein a relationship between an input torque and an output torque in said mechanical model is expressed as the following equation:

    τ*(t)=J.sub.n ·ω(t)+.increment.J(t)·ω(t)+J(t)·.omega.(t)+F.sub.f +T.sub.d

in which τ*(t) is an input torque, J_(n) is a nominal moment of inertia, ω(t) is an angular velocity, J(t) is a moment of inertia which is time-varying according to a load, .increment.J(t) is expressed as J_(n) -J(t), F_(f) is a frictional torque and T_(d) is a disturbance torque.
 6. A velocity control method for use in a servo motor according to claim 3, wherein a relationship between an input torque and an output torque in said mechanical model is expressed as the following equation:

    τ*(t)=J.sub.n ·ω(t)+.increment.J(t)·ω(t)+J(t)·.omega.(t)+F.sub.f +T.sub.d

in which τ*(t) is an input torque, J_(n) is a nominal moment of inertia, ω(t) is an angular velocity, J(t) is a moment of inertia which is time-varying according to a load, .increment.J(t) is expressed as J_(n) -J(t), F_(f) is a frictional torque and T_(d) is a disturbance torque.
 7. A velocity control method for use in a servo motor according to claim 1, wherein the estimation velocity in said virtual model is expressed as the following equation: ##EQU7## in which ω(s) is an estimation velocity, T_(u) (s) is a control torque, and s is a Laplacian variable.
 8. A velocity control method for use in a servo motor according to claim 2, wherein the estimation velocity in said virtual model is expressed as the following equation: ##EQU8## in which ω(s) is an estimation velocity, T_(u) (s) is a control torque, and s is a Laplacian variable.
 9. A velocity control method for use in a servo motor according to claim 3, wherein the estimation velocity in said virtual model is expressed as the following equation: ##EQU9## in which ω(s) is an estimation velocity, T_(u) (s) is a control torque, and s is a Laplacian variable. 